#pragma once
#include <opencv2\opencv.hpp>

class Knife
{
public:
	float knifeLength;
	float armLength;
	

public:
	Knife()
	{
		knifeLength = 0.027;

		//knifeLength += 0.015;

		//knifeLength += 0.115;

		knifeLength += 0.245;

		armLength = 0.03;

		//armLength += 0.288;

		armLength += 0.175;

		//knifeLength = 0.165+0.027;
		//knifeLength = 0.015 + 0.027;
		//armLength = 0.03+0.165-0.017;
	}

	void GetHeadCoordinate(cv::Vec3f pt[4])
	{
		float xoff = 0;

		pt[0] = cv::Vec3f(xoff, 0, 0);
		pt[1] = cv::Vec3f(xoff, 0, armLength);
		pt[2] = cv::Vec3f(xoff, knifeLength, armLength);
		pt[3] = cv::Vec3f(1 + xoff, 0, 0);

		cv::Vec3f rv(0, 0, CV_PI*(0.25));
		cv::Matx33f rm;
		cv::Rodrigues(rv, rm);

		pt[0] = rm*pt[0];
		pt[1] = rm*pt[1];
		pt[2] = rm*pt[2];
		pt[3] = rm*pt[3];

		std::cout << "GetHeadCoordinate=" << pt[0] << '\n'
			<< pt[1] << '\n'
			<< pt[2] << '\n'
			<< pt[3] << '\n'
			<< '\n';
	}

	cv::Vec3f ToBase(cv::Vec3f xyz, cv::Vec3f rxyz)
	{
		cv::Matx33f rm;
		cv::Rodrigues(rxyz, rm);

		cv::Vec3f pt[4];
		GetHeadCoordinate(pt);
		return xyz + rm*pt[2];
	}


	//length of knifeDirection = 1, from pt[2] to pt[1]
	void GetBaseCoordinate(cv::Vec3f pt[4], cv::Vec3f target, cv::Vec3f knifeDirection)
	{
		pt[2] = target;
		pt[1] = target + knifeLength*knifeDirection;

		pt[3] = knifeDirection.cross(cv::Vec3f(0, 0, 1));

		double Apt3 = cv::norm(pt[3]);
		pt[3] /= Apt3;

		cv::Vec3f armDirection = pt[3].cross(knifeDirection);
#if 1
		pt[0] = pt[1] + armLength*armDirection;
		pt[3] = pt[0] + pt[3];
#endif
#if 0
		pt[0] = pt[1] - armLength*armDirection;
		pt[3] = pt[0] - pt[3];

#endif

	}



	static void ComputeTransform(cv::Vec3f &xyz, cv::Vec3f &rxyz, const cv::Vec3f ptHead[4], const cv::Vec3f ptBase[4])
	{
		cv::Vec3f ctHead = (ptHead[0] + ptHead[1] + ptHead[2] + ptHead[3]) / 4;
		cv::Vec3f ctBase = (ptBase[0] + ptBase[1] + ptBase[2] + ptBase[3]) / 4;
		cv::Matx33f H = (ptHead[0] - ctHead)*((ptBase[0] - ctBase).t());
		H += (ptHead[1] - ctHead)*((ptBase[1] - ctBase).t());
		H += (ptHead[2] - ctHead)*((ptBase[2] - ctBase).t());
		H += (ptHead[3] - ctHead)*((ptBase[3] - ctBase).t());
		cv::Mat U, S, Vt;
		cv::SVDecomp(H, S, U, Vt);
		cv::Mat R0 = (U*Vt).t();
		cv::Matx33f R((float*)R0.ptr());
		cv::Rodrigues(R, rxyz);
		xyz = ctBase - R*ctHead;
	}

	static cv::Vec3f NormalOrient()
	{
		cv::Vec3f ph[4] = {
			cv::Vec3f(0,0,0),
			cv::Vec3f(1,0,0),
			cv::Vec3f(0,1,0),
			cv::Vec3f(0,0,1)
		};

		double q2 = sqrt(2.);

		cv::Vec3f pb[4] = {
			cv::Vec3f(0,0,0),
			cv::Vec3f(-1 / q2,1 / q2,0),
			cv::Vec3f(1 / q2,1 / q2,0),
			cv::Vec3f(0,0,-1)
		};

		cv::Vec3f rt[2];
		ComputeTransform(rt[0], rt[1], ph, pb);
		return rt[1];
	}

	void GetArmPos(cv::Vec3f &xyz, cv::Vec3f &rxyz, cv::Vec3f target, cv::Vec3f knifeDirection)
	{
		cv::Vec3f ptHead[4];
		GetHeadCoordinate(ptHead);
		cv::Vec3f ptBase[4];
		GetBaseCoordinate(ptBase, target, knifeDirection);
		ComputeTransform(xyz, rxyz, ptHead, ptBase);
	}


	void GetArmPos(cv::Vec6f &pos, cv::Vec3f target, float pitch = CV_PI*0.08, float yaw=CV_PI*1.1)
	{
		cv::Vec3f knifeDirection(cos(pitch)*cos(yaw), cos(pitch)*sin(yaw), sin(pitch));
		cv::Vec3f tmp[2];
		GetArmPos(tmp[0], tmp[1], target, knifeDirection);
		pos = cv::Vec6f(tmp[0][0], tmp[0][1], tmp[0][2], tmp[1][0], tmp[1][1], tmp[1][2]);
	}



	void OneCut(cv::Vec2f start, cv::Vec2f end, std::vector<cv::Vec6f> &position, float z = 0.04, float pitch = CV_PI / 6)
	{
		cv::Vec3f target[2];
		cv::Vec3f knifeDirection;
		target[0] = cv::Vec3f(start[0], start[1], z);
		target[1] = cv::Vec3f(end[0], end[1], z);
		knifeDirection = target[1] - target[0];
		cv::Vec3f rv = knifeDirection.cross(cv::Vec3f(0, 0, 1));
		double l = cv::norm(rv);
		rv = rv*pitch / l;
		cv::Matx33f rm;
		cv::Rodrigues(rv, rm);
		knifeDirection = rm*knifeDirection;
		
		l = cv::norm(knifeDirection);
		knifeDirection = knifeDirection / l;

		cv::Vec3f pos[4];

		GetArmPos(pos[0], pos[1], target[0], knifeDirection);
		GetArmPos(pos[2], pos[3], target[1], knifeDirection);

		position.push_back(cv::Vec6f(pos[0][0], pos[0][1], pos[0][2], pos[1][0], pos[1][1], pos[1][2]));
		position.push_back(cv::Vec6f(pos[2][0], pos[2][1], pos[2][2], pos[3][0], pos[3][1], pos[3][2]));
	}

	void CrossCut(cv::Vec2f rect[4], std::vector<cv::Vec6f> &position, float z = 0.04, float pitch = CV_PI / 6)
	{
		OneCut((rect[0] + rect[1]) / 2, (rect[2] + rect[3]) / 2, position, z, pitch);
		OneCut((rect[2] + rect[1]) / 2, (rect[0] + rect[3]) / 2, position, z, pitch);
	}


	void CutCake(cv::Vec2f rect[4], std::vector<cv::Vec6f> &path, float zDown = 0.01, float zUp = 0.1, float pitch = CV_PI*0.02)
	{
		cv::Vec2f edge[2];

		double el[2] = { cv::norm(rect[0] - rect[1]), cv::norm(rect[0] - rect[3]) };
		if (el[0] > el[1])
		{
			edge[1] = (rect[0] + rect[1]) / 2;
			edge[0] = (rect[2] + rect[3]) / 2;
		}
		else
		{
			edge[1] = (rect[0] + rect[3]) / 2;
			edge[0] = (rect[2] + rect[1]) / 2;
		}

		cv::Vec2f knifeYaw = edge[1] - edge[0];

		double yaw = atan2(knifeYaw[1], knifeYaw[0]);

		cv::Vec6f pos;

		GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zUp), pitch, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zDown), pitch, yaw);
		path.push_back(pos);

		//GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zUp), pitch, yaw);
		//path.push_back(pos);

		//GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zDown), pitch, yaw);
		//path.push_back(pos);


		GetArmPos(pos, cv::Vec3f(edge[1][0], edge[1][1], zDown), pitch, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[1][0], edge[1][1], zUp), pitch, yaw);
		path.push_back(pos);

	
	}


	void CutCakeA(cv::Vec2f rect[4], std::vector<cv::Vec6f> &path, float z = 0.02, float pitchUp = CV_PI*0.24, float pitchDown = CV_PI*0)
	{
		cv::Vec2f edge[2];

		double el[2] = { cv::norm(rect[0] - rect[1]), cv::norm(rect[0] - rect[3]) };
		if (el[0] > el[1])
		{
			edge[1] = (rect[0] + rect[1]) / 2;
			edge[0] = (rect[2] + rect[3]) / 2;
		}
		else
		{
			edge[1] = (rect[0] + rect[3]) / 2;
			edge[0] = (rect[2] + rect[1]) / 2;
		}

		cv::Vec2f knifeYaw = edge[1] - edge[0];

		double yaw = atan2(knifeYaw[1], knifeYaw[0]);

		cv::Vec6f pos;

		GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], z), pitchUp, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], z), pitchDown, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], z), pitchUp, yaw);
		path.push_back(pos);
	}

	void CutCakeB(cv::Vec2f rect[4], std::vector<cv::Vec6f> &path, float zDown = 0.01, float zUp = 0.08, float pitch = CV_PI*0.48, float interval = 0.01)
	{
		cv::Vec2f edge[2];

		double el[2] = { cv::norm(rect[0] - rect[1]), cv::norm(rect[0] - rect[3]) };
		if (el[0] > el[1])
		{
			edge[1] = (rect[0] + rect[1]) / 2;
			edge[0] = (rect[2] + rect[3]) / 2;
		}
		else
		{
			edge[1] = (rect[0] + rect[3]) / 2;
			edge[0] = (rect[2] + rect[1]) / 2;
		}

		cv::Vec2f knifeYaw = edge[1] - edge[0];

		double yaw = atan2(knifeYaw[1], knifeYaw[0]);

		double l = cv::norm(knifeYaw);

		knifeYaw = knifeYaw*(interval / l);

		for (int i = 0;(i - 1)*interval < l;i++)
		{
		cv::Vec6f pos;

		GetArmPos(pos, cv::Vec3f(edge[0][0]+ knifeYaw[0]*i, edge[0][1]+ knifeYaw[1]*i, zUp), pitch, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[0][0] + knifeYaw[0]*i, edge[0][1] + knifeYaw[1]*i, zDown), pitch, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[0][0] + knifeYaw[0]*i, edge[0][1] + knifeYaw[1]*i, zUp), pitch, yaw);
		path.push_back(pos);
		}

		
	}


	void CutCakeC(cv::Vec2f rect[4], std::vector<cv::Vec6f> &path, float zDown = 0.01, float zUp = 0.08, float pitch = CV_PI*0.48, float interval = 0.01)
	{
		cv::Vec2f edge[2];

		double el[2] = { cv::norm(rect[0] - rect[1]), cv::norm(rect[0] - rect[3]) };
		if (el[0] > el[1])
		{
			edge[1] = (rect[0] + rect[1]) / 2;
			edge[0] = (rect[2] + rect[3]) / 2;
		}
		else
		{
			edge[1] = (rect[0] + rect[3]) / 2;
			edge[0] = (rect[2] + rect[1]) / 2;
		}


		cv::Vec2f knifeYaw = edge[1] - edge[0];

		double yaw = atan2(knifeYaw[1], knifeYaw[0]);

		double l = cv::norm(knifeYaw);

		knifeYaw = knifeYaw*(interval / l);

		cv::Vec2f center = (edge[0] + edge[1]) / 2;

		cv::Vec6f pos;

		//GetArmPos(pos, cv::Vec3f(center[0], center[1], zUp), pitch, yaw);
		//path.push_back(pos);

		//GetArmPos(pos, cv::Vec3f(center[0], center[1], zDown), pitch, yaw);
		//path.push_back(pos);

		//GetArmPos(pos, cv::Vec3f(center[0], center[1], zUp), pitch, yaw);
		//path.push_back(pos);

		int n = l / interval / 2;
		n = 2;

		for (int i = -1; i < n;i++)
		{	
			cv::Vec2f pos2 = center + knifeYaw*i;

			GetArmPos(pos, cv::Vec3f(pos2[0], pos2[1], zUp), pitch, yaw);
			path.push_back(pos);

			GetArmPos(pos, cv::Vec3f(pos2[0], pos2[1], zDown), pitch, yaw);
			path.push_back(pos);

			GetArmPos(pos, cv::Vec3f(pos2[0], pos2[1], zUp), pitch, yaw);
			path.push_back(pos);

			//pos2 = center + knifeYaw*i;

			//GetArmPos(pos, cv::Vec3f(pos2[0], pos2[1], zUp), pitch, yaw);
			//path.push_back(pos);

			//GetArmPos(pos, cv::Vec3f(pos2[0], pos2[1], zDown), pitch, yaw);
			//path.push_back(pos);

			//GetArmPos(pos, cv::Vec3f(pos2[0], pos2[1], zUp), pitch, yaw);
			//path.push_back(pos);
		}


	}



	void CutCakeD(cv::Vec2f rect[4], std::vector<cv::Vec6f> &path, float zDown = 0.002, float zUp = 0.06, float pitch = CV_PI*0.02)
	{
		float r = 0.4;

		cv::Vec2f edge[4];

		double el[2] = { cv::norm(rect[0] - rect[1]), cv::norm(rect[0] - rect[3]) };
		if (el[0] > el[1])
		{
			edge[1] = rect[0]*r + rect[1]*(1-r);
			edge[0] = rect[3]*r + rect[2]*(1-r);

			edge[3] = rect[0]*(1-r) + rect[1] *r;
			edge[2] = (rect[3]* (1 - r) + rect[2] * r) ;

		}
		else
		{
			edge[1] = (rect[0] * r + rect[3] * (1 - r)) ;
			edge[0] = (rect[1] * r + rect[2] * (1 - r)) ;

			edge[3] = (rect[0] * (1 - r) + rect[3] * r);
			edge[2] = (rect[1] * (1 - r) + rect[2] * r);
		}

		{
			cv::Vec2f knifeYaw = edge[1] - edge[0];

		double yaw = atan2(knifeYaw[1], knifeYaw[0]);

		cv::Vec6f pos;

		GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zUp), pitch, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zDown), pitch, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zUp), pitch, yaw);
		path.push_back(pos);

		}

		{
		cv::Vec2f knifeYaw = edge[3] - edge[2];

		double yaw = atan2(knifeYaw[1], knifeYaw[0]);

		cv::Vec6f pos;

		GetArmPos(pos, cv::Vec3f(edge[2][0], edge[2][1], zUp), pitch, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[2][0], edge[2][1], zDown), pitch, yaw);
		path.push_back(pos);

		GetArmPos(pos, cv::Vec3f(edge[2][0], edge[2][1], zUp), pitch, yaw);
		path.push_back(pos);
		}

	}



	void CutCakeE(cv::Vec2f rect[4], std::vector<cv::Vec6f> &path, float zDown = -0.025, float zUp = 0.08, float pitch = CV_PI*0.05, float interval = CV_PI*0.15)
	{
		cv::Vec2f edge[2];

		double el[2] = { cv::norm(rect[0] - rect[1]), cv::norm(rect[0] - rect[3]) };
		if (el[0] > el[1])
		{
			edge[1] = (rect[0] + rect[1]) / 2;
			edge[0] = (rect[2] + rect[3]) / 2;
		}
		else
		{
			edge[1] = (rect[0] + rect[3]) / 2;
			edge[0] = (rect[2] + rect[1]) / 2;
		}

		cv::Vec2f knifeYaw = edge[1] - edge[0];

		cv::Vec2f center = (edge[1] + edge[0]) / 2;

		double yaw = atan2(knifeYaw[1], knifeYaw[0]);

		cv::Vec2f knifeYaw2 = knifeYaw / 2;

		//cv::Matx22f rm(cos(interval), sin(interval), -sin(interval), cos(interval));

		for (int i = -1; i<2; i++)
		{
			cv::Vec6f pos;

			cv::Matx22f rm(cos(interval*i), sin(interval*i), -sin(interval*i), cos(interval*i));

			cv::Vec2f knifeYaw2a = rm*knifeYaw2;

			cv::Vec2f target = center - knifeYaw2a;
			
			GetArmPos(pos, cv::Vec3f(target[0], target[1], zUp), pitch, yaw- interval*i);
			path.push_back(pos);

			GetArmPos(pos, cv::Vec3f(target[0], target[1], zDown), pitch, yaw- interval*i);
			path.push_back(pos);

			GetArmPos(pos, cv::Vec3f(target[0], target[1], zUp), pitch, yaw- interval*i);
			path.push_back(pos);
		}


	}

	void CutCakeF(cv::Vec2f rect[4], std::vector<cv::Vec6f> &path, float zDown = -0.025, float zUp = 0.08, float pitch = CV_PI*0.05, float r = 0.4)
	{
		cv::Vec2f edge[2];

		double el[2] = { cv::norm(rect[0] - rect[1]), cv::norm(rect[0] - rect[3]) };
		if (el[0] > el[1])
		{
			edge[1] = rect[0] * r + rect[1] * (1 - r);
			edge[0] = rect[3] * r + rect[2] * (1 - r);
		}
		else
		{
			edge[1] = (rect[0] * r + rect[3] * (1 - r));
			edge[0] = (rect[1] * r + rect[2] * (1 - r));
		}

		{
			cv::Vec2f knifeYaw = edge[1] - edge[0];

			double yaw = atan2(knifeYaw[1], knifeYaw[0]);

			cv::Vec6f pos;

			GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zUp), pitch, yaw);
			path.push_back(pos);

			GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zDown), pitch, yaw);
			path.push_back(pos);

			GetArmPos(pos, cv::Vec3f(edge[0][0], edge[0][1], zUp), pitch, yaw);
			path.push_back(pos);

		}	
	}


	void CutCakeG(cv::Vec2f rect[4], std::vector<cv::Vec6f> &path, float zDown = 0.07, float zUp = 0.09, float pitch = CV_PI*0.05)
	{
		CutCakeF(rect, path, zDown, zUp, pitch, 1. / 6);
		CutCakeF(rect, path, zDown, zUp, pitch, 2. / 6);
		CutCakeF(rect, path, zDown, zUp, pitch, 3. / 6);
		CutCakeF(rect, path, zDown, zUp, pitch, 4. / 6);
		CutCakeF(rect, path, zDown, zUp, pitch, 5. / 6);
	}


};
